#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <signal.h>

#include "ethercattype.h"
#include "nicdrv.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatdc.h"
#include "ethercatcoe.h"
#include "ethercatfoe.h"
#include "ethercatconfig.h"
#include "ethercatprint.h"
#include "math.h"
#include <alchemy/task.h> 
#include <alchemy/timer.h> 

#define TASK_PRIO 99 /* Highest RT priority */ 
#define TASK_MODE 0 /* No flags */ 
#define TASK_STKSZ 0/* Stack size (use default one) */ 
#define TASK_PERIOD  1000000  /* 100 usc period */
#define NSEC_PER_SEC 1000000000
 RT_TASK task_desc; 

char IOmap[4096];
uint16 control;

uint16 control_world;
uint16 state;
int32 curr_position;
int32 position=0;
int32 position2;
int add_position;
RTIME tt=0;
int64 toff,gl_delta;
// 使从站进入op状态
void slavetop(int i)
{
       
	ec_slave[i].state = EC_STATE_OPERATIONAL;
	ec_send_processdata();
	ec_receive_processdata(EC_TIMEOUTRET);
	ec_writestate(0);

}
//将位置数据添加倒主站pdo的内存中
void position_to_add(int32 position)
{
	
	ec_slave[1].outputs[0x0002] = position;
	ec_slave[1].outputs[0x0003] = (position>>8)&0xff;
	ec_slave[1].outputs[0x0004] = (position>>16)&0xff;
	ec_slave[1].outputs[0x0005] = (position>>24)&0xff;
        ec_slave[1].outputs[0x000E] = position;
	ec_slave[1].outputs[0x000F] = (position>>8)&0xff;
	ec_slave[1].outputs[0x0010] = (position>>16)&0xff;
	ec_slave[1].outputs[0x0011] = (position>>24)&0xff;
        
	
}
//用pid算法对主站时钟进行动态补偿
void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime)
{
   static int64 integral = 0;
  
   int64 delta;   
   /* set linux sync point 50us later than DC sync, just as example */
   delta = (reftime-500000) % cycletime;
   if(delta>(cycletime / 2)) 
   { delta= delta - cycletime; }
  *offsettime = -(delta /20) - (integral /10000);
   integral=integral+delta;
   //d=d-delta; 
  //rt_printf("%lld\n",delta); 
} 

void add_timespec(struct timespec *ts, int64 addtime)
{
   int64 sec, nsec;
   
   nsec = addtime % NSEC_PER_SEC;
   sec = (addtime - nsec) / NSEC_PER_SEC;
   ts->tv_sec += sec;
   ts->tv_nsec += nsec;
   if ( ts->tv_nsec > NSEC_PER_SEC ) 
   { 
      nsec = ts->tv_nsec % NSEC_PER_SEC;
      ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
      ts->tv_nsec = nsec;
   }   
}   
//从站使能函数 
void enablefun(void)
{
   static int enable=0;
  
 
  if(enable<2000)
  {
          
                  ec_slave[1].outputs[0x0000] = 0x00;
                  ec_slave[1].outputs[0x0013] = 0x00;
                  ec_slave[1].outputs[0x001E] = 0x08;    
                
          
       
          if(enable>500)
          {
              
                        ec_slave[1].outputs[0x0000] = 0x06;
                       
                    
                        ec_slave[1].outputs[0x000c] = 0x06;
                           
                
          }
       
    
          if(enable>1000)
            {
                        ec_slave[1].outputs[0x0000] = 0x07;
                    
                        ec_slave[1].outputs[0x000c] = 0x07;    
                  
            }
     
    
           if(enable>1500)
           {
             
                        ec_slave[1].outputs[0x0000] = 0x0f;
                      
                        ec_slave[1].outputs[0x000c] = 0x0f;
                        
           }
      enable++;
     rt_printf("%d\n",enable);  
  }   
  else
  {
     static float t=0;
     position=(int32)(50000*sin(t));
     t=t+0.001;
     position_to_add(position);
      
      
  }     
     
   
}

void simpletest(void *arg)
{
        
        char* ifname="rteth0";
	if(ec_init(ifname))
 {
	   rt_printf("start ethernet at %s\n",ifname);
	   if ( ec_config_init(FALSE) > 0 )
	{               
		   	 rt_printf("found %d slave on the bus\n",ec_slavecount);            
		          ec_config_map(&IOmap);
                          rt_printf("slave\n");
                          ec_configdc();
          //设置从站的dc同步参数，开启dc同步
           for(int i =1;i<=ec_slavecount;i++)
          {   
             ec_dcsync0(i, TRUE,1000000,200000);
                 
          }
           

        for(int i=0;i<ec_slavecount;i++)
        {     
            slavetop(i); 
            rt_printf("slave%d to op\n", i);                   
        }
        if((ec_slave[0].state == EC_STATE_OPERATIONAL)&&(ec_slave[1].state == EC_STATE_OPERATIONAL))
      {
                          struct timespec   ts;
                          //获取系统时间
                          clock_gettime(CLOCK_MONOTONIC, &ts);
                          //设置补偿时间初始值为0
                          toff = 0;                             
          while(1)
		        {  
                  //根据计算出的补偿时间修改系统的时间
                  add_timespec(&ts, TASK_PERIOD + toff);
                  //根据系统时间让主站的时钟加快或者减慢
                  clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts,NULL);
                  //系统使能并发送位置
                   enablefun();
                  //计算出补偿时间
                  
                  ec_sync(ec_DCtime,1000000,&toff);
                  //把主站内存的数据发送倒从站
                  ec_send_processdata();
                  //把从站内存的数据接收放到从站
                  ec_receive_processdata(EC_TIMEOUTRET);
               }
	   }
			
   }
	
 }
 
		

}

int creat_task(void)
{
                  
            int err; 
             /* Create a real-time task */ 
            int err_create =rt_task_create(&task_desc,"task_desc",TASK_STKSZ,TASK_PRIO,TASK_MODE); 
              if (err_create!=0)
			  {
				  printf("创建线程错误");
			  } 
             /* If successfully created, start thetask. */ 
           int err_start=rt_task_start(&task_desc,&simpletest,NULL); 
		   if (err_start!=0)
		   {
			  printf("开启任务失败");
		   }
		   if((err_start||err_create)==0)
		   {
              return 0;
		   }
		   else
		   {
			   return -1;
		   }
	 
	      
           
}
int main(int argc, char *argv[])
{
       
	printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
	
    
	if (argc > 1)
	{      
      int error=creat_task();
      if(error<0)
		{
			printf("创建xenomai建成错误");
		}	   
	}
	else
	{
		printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
	}   
      pause();
	printf("End program\n");
	return 0;
}


